/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                         License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2009, PhaseSpace Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The names of the copyright holders may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"
#include "opencv2/calib3d/calib3d.hpp"

namespace cv {

LevMarqSparse::LevMarqSparse() {
    A = B = W = Vis_index = X = prevP = P = deltaP = err = JtJ_diag = S = hX = NULL;
    U = ea = V = inv_V_star = eb = Yj = NULL;
}

LevMarqSparse::~LevMarqSparse() {
    clear();
}

LevMarqSparse::LevMarqSparse(int npoints, // number of points
                             int ncameras, // number of cameras
                             int nPointParams, // number of params per one point  (3 in case of 3D points)
                             int nCameraParams, // number of parameters per one camera
                             int nErrParams, // number of parameters in measurement vector
                             // for 1 point at one camera (2 in case of 2D projections)
                             Mat& visibility, // visibility matrix. rows correspond to points, columns correspond to cameras
                             // 1 - point is visible for the camera, 0 - invisible
                             Mat& P0, // starting vector of parameters, first cameras then points
                             Mat& X_, // measurements, in order of visibility. non visible cases are skipped
                             TermCriteria criteria, // termination criteria

                             // callback for estimation of Jacobian matrices
                             void (CV_CDECL* fjac)(int i, int j, Mat& point_params,
                                     Mat& cam_params, Mat& A, Mat& B, void* data),
                             // callback for estimation of backprojection errors
                             void (CV_CDECL* func)(int i, int j, Mat& point_params,
                                     Mat& cam_params, Mat& estim, void* data),
                             void* data // user-specific data passed to the callbacks
                            ) {
    A = B = W = Vis_index = X = prevP = P = deltaP = err = JtJ_diag = S = hX = NULL;
    U = ea = V = inv_V_star = eb = Yj = NULL;

    run(npoints, ncameras, nPointParams, nCameraParams, nErrParams, visibility,
        P0, X_, criteria, fjac, func, data);
}

void LevMarqSparse::clear() {
    for (int i = 0; i < num_points; i++) {
        for (int j = 0; j < num_cams; j++) {
            CvMat* tmp = ((CvMat**)(A->data.ptr + i * A->step))[j];
            if (tmp) {
                cvReleaseMat(&tmp);
            }

            tmp = ((CvMat**)(B->data.ptr + i * B->step))[j];
            if (tmp) {
                cvReleaseMat(&tmp);
            }

            tmp = ((CvMat**)(W->data.ptr + j * W->step))[i];
            if (tmp) {
                cvReleaseMat(&tmp);
            }
        }
    }
    cvReleaseMat(&A);
    cvReleaseMat(&B);
    cvReleaseMat(&W);
    cvReleaseMat(&Vis_index);

    for (int j = 0; j < num_cams; j++) {
        cvReleaseMat(&U[j]);
    }
    delete U;

    for (int j = 0; j < num_cams; j++) {
        cvReleaseMat(&ea[j]);
    }
    delete ea;

    //allocate V and inv_V_star
    for (int i = 0; i < num_points; i++) {
        cvReleaseMat(&V[i]);
        cvReleaseMat(&inv_V_star[i]);
    }
    delete V;
    delete inv_V_star;

    for (int i = 0; i < num_points; i++) {
        cvReleaseMat(&eb[i]);
    }
    delete eb;

    for (int i = 0; i < num_points; i++) {
        cvReleaseMat(&Yj[i]);
    }
    delete Yj;

    cvReleaseMat(&X);
    cvReleaseMat(&prevP);
    cvReleaseMat(&P);
    cvReleaseMat(&deltaP);

    cvReleaseMat(&err);

    cvReleaseMat(&JtJ_diag);
    cvReleaseMat(&S);
    cvReleaseMat(&hX);
}

//A params correspond to  Cameras
//B params correspont to  Points

//num_cameras  - total number of cameras
//num_points   - total number of points

//num_par_per_camera - number of parameters per camera
//num_par_per_point - number of parameters per point

//num_errors - number of measurements.

void LevMarqSparse::run(int num_points_,  //number of points
                        int num_cams_, //number of cameras
                        int num_point_param_, //number of params per one point  (3 in case of 3D points)
                        int num_cam_param_, //number of parameters per one camera
                        int num_err_param_, //number of parameters in measurement vector for 1 point at one camera (2 in case of 2D projections)
                        Mat& visibility,   //visibility matrix . rows correspond to points, columns correspond to cameras
                        // 0 - point is visible for the camera, 0 - invisible
                        Mat& P0, //starting vector of parameters, first cameras then points
                        Mat& X_init, //measurements, in order of visibility. non visible cases are skipped
                        TermCriteria criteria_init,
                        void (*fjac_)(int i, int j, Mat& point_params, Mat& cam_params, Mat& A, Mat& B, void* data),
                        void (*func_)(int i, int j, Mat& point_params, Mat& cam_params, Mat& estim, void* data),
                        void* data_
                       ) { //termination criteria
    clear();

    func = func_; //assign evaluation function
    fjac = fjac_; //assign jacobian
    data = data_;

    num_cams = num_cams_;
    num_points = num_points_;
    num_err_param = num_err_param_;
    num_cam_param = num_cam_param_;
    num_point_param = num_point_param_;

    //compute all sizes
    int Aij_width = num_cam_param;
    int Aij_height = num_err_param;

    int Bij_width = num_point_param;
    int Bij_height = num_err_param;

    int U_size = Aij_width;
    int V_size = Bij_width;

    int Wij_height = Aij_width;
    int Wij_width = Bij_width;

    //allocate memory for all Aij, Bij, U, V, W

    //allocate num_points*num_cams matrices A

    //Allocate matrix A whose elements are nointers to Aij
    //if Aij is zero (point i is not visible in camera j) then A(i,j) contains NULL
    A = cvCreateMat(num_points, num_cams, CV_32S /*pointer is stored here*/);
    B = cvCreateMat(num_points, num_cams, CV_32S /*pointer is stored here*/);
    W = cvCreateMat(num_cams, num_points, CV_32S /*pointer is stored here*/);
    Vis_index = cvCreateMat(num_points, num_cams, CV_32S /*integer index is stored here*/);
    cvSetZero(A);
    cvSetZero(B);
    cvSetZero(W);
    cvSet(Vis_index, cvScalar(-1));

    //fill matrices A and B based on visibility
    CvMat _vis = visibility;
    int index = 0;
    for (int i = 0; i < num_points; i++) {
        for (int j = 0; j < num_cams; j++) {
            if (((int*)(_vis.data.ptr + i * _vis.step))[j]) {
                ((int*)(Vis_index->data.ptr + i * Vis_index->step))[j] = index;
                index += num_err_param;

                //create matrices Aij, Bij
                CvMat* tmp = cvCreateMat(Aij_height, Aij_width, CV_64F);
                ((CvMat**)(A->data.ptr + i * A->step))[j] = tmp;

                tmp = cvCreateMat(Bij_height, Bij_width, CV_64F);
                ((CvMat**)(B->data.ptr + i * B->step))[j] = tmp;

                tmp = cvCreateMat(Wij_height, Wij_width, CV_64F);
                ((CvMat**)(W->data.ptr + j * W->step))[i] = tmp;  //note indices i and j swapped
            }
        }
    }

    //allocate U
    U = new CvMat* [num_cams];
    for (int j = 0; j < num_cams; j++) {
        U[j] = cvCreateMat(U_size, U_size, CV_64F);
    }
    //allocate ea
    ea = new CvMat* [num_cams];
    for (int j = 0; j < num_cams; j++) {
        ea[j] = cvCreateMat(U_size, 1, CV_64F);
    }

    //allocate V and inv_V_star
    V = new CvMat* [num_points];
    inv_V_star = new CvMat* [num_points];
    for (int i = 0; i < num_points; i++) {
        V[i] = cvCreateMat(V_size, V_size, CV_64F);
        inv_V_star[i] = cvCreateMat(V_size, V_size, CV_64F);
    }

    //allocate eb
    eb = new CvMat* [num_points];
    for (int i = 0; i < num_points; i++) {
        eb[i] = cvCreateMat(V_size, 1, CV_64F);
    }

    //allocate Yj
    Yj = new CvMat* [num_points];
    for (int i = 0; i < num_points; i++) {
        Yj[i] = cvCreateMat(Wij_height, Wij_width, CV_64F);    //Yij has the same size as Wij
    }

    //allocate matrix S
    S = cvCreateMat(num_cams * num_cam_param, num_cams * num_cam_param, CV_64F);

    JtJ_diag = cvCreateMat(num_cams * num_cam_param + num_points * num_point_param, 1, CV_64F);

    //set starting parameters
    CvMat _tmp_ = CvMat(P0);
    prevP = cvCloneMat(&_tmp_);
    P = cvCloneMat(&_tmp_);
    deltaP = cvCloneMat(&_tmp_);

    //set measurements
    _tmp_ = CvMat(X_init);
    X = cvCloneMat(&_tmp_);
    //create vector for estimated measurements
    hX = cvCreateMat(X->rows, X->cols, CV_64F);
    //create error vector
    err = cvCreateMat(X->rows, X->cols, CV_64F);

    ask_for_proj();

    //compute initial error
    cvSub(X, hX, err);

    prevErrNorm = cvNorm(err, 0,  CV_L2);
    iters = 0;
    criteria = criteria_init;

    optimize();
}

void LevMarqSparse::ask_for_proj() {
    //given parameter P, compute measurement hX
    int ind = 0;
    for (int i = 0; i < num_points; i++) {
        CvMat point_mat;
        cvGetSubRect(P, &point_mat, cvRect(0, num_cams * num_cam_param + num_point_param * i, 1, num_point_param));

        for (int j = 0; j < num_cams; j++) {
            CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
            if (Aij) {   //visible
                CvMat cam_mat;
                cvGetSubRect(P, &cam_mat, cvRect(0, j * num_cam_param, 1, num_cam_param));
                CvMat measur_mat;
                cvGetSubRect(hX, &measur_mat, cvRect(0, ind * num_err_param, 1, num_err_param));
                Mat _point_mat(&point_mat), _cam_mat(&cam_mat), _measur_mat(&measur_mat);
                func(i, j, _point_mat, _cam_mat, _measur_mat, data);

                assert(ind* num_err_param == ((int*)(Vis_index->data.ptr + i * Vis_index->step))[j]);

                ind += 1;

            }
        }
    }
}
//iteratively asks for Jacobians for every camera_point pair
void LevMarqSparse::ask_for_projac() { //should be evaluated at point prevP
    // compute jacobians Aij and Bij
    for (int i = 0; i < A->height; i++) {
        CvMat point_mat;
        cvGetSubRect(prevP, &point_mat, cvRect(0, num_cams * num_cam_param + num_point_param * i, 1, num_point_param));


        CvMat** A_line = (CvMat**)(A->data.ptr + A->step * i);
        CvMat** B_line = (CvMat**)(B->data.ptr + B->step * i);

        for (int j = 0; j < A->width; j++) {
            CvMat* Aij = A_line[j];
            if (Aij) {   //Aij is not zero
                CvMat cam_mat;
                cvGetSubRect(prevP, &cam_mat, cvRect(0, j * num_cam_param, 1, num_cam_param));

                CvMat* Bij = B_line[j];
                Mat _point_mat(&point_mat), _cam_mat(&cam_mat), _Aij(Aij), _Bij(Bij);
                (*fjac)(i, j, _point_mat, _cam_mat, _Aij, _Bij, data);
            }
        }
    }
}

void LevMarqSparse::optimize() { //main function that runs minimization
    bool done = false;

    CvMat* YWt = cvCreateMat(num_cam_param, num_cam_param, CV_64F);   //this matrix used to store Yij*Wik'
    CvMat* E = cvCreateMat(S->height, 1 , CV_64F);   //this is right part of system with S

    while (!done) {
        // compute jacobians Aij and Bij
        ask_for_projac();

        //compute U_j  and  ea_j
        for (int j = 0; j < num_cams; j++) {
            cvSetZero(U[j]);
            cvSetZero(ea[j]);
            //summ by i (number of points)
            for (int i = 0; i < num_points; i++) {
                //get Aij
                CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
                if (Aij) {
                    //Uj+= AijT*Aij
                    cvGEMM(Aij, Aij, 1, U[j], 1, U[j], CV_GEMM_A_T);

                    //ea_j += AijT * e_ij
                    CvMat eij;

                    int index = ((int*)(Vis_index->data.ptr + i * Vis_index->step))[j];

                    cvGetSubRect(err, &eij, cvRect(0, index, 1, Aij->height /*width of transposed Aij*/));
                    cvGEMM(Aij, &eij, 1, ea[j], 1, ea[j], CV_GEMM_A_T);
                }
            }
        } //U_j and ea_j computed for all j

        //compute V_i  and  eb_i
        for (int i = 0; i < num_points; i++) {
            cvSetZero(V[i]);
            cvSetZero(eb[i]);

            //summ by i (number of points)
            for (int j = 0; j < num_cams; j++) {
                //get Bij
                CvMat* Bij = ((CvMat**)(B->data.ptr + B->step * i))[j];

                if (Bij) {
                    //Vi+= BijT*Bij
                    cvGEMM(Bij, Bij, 1, V[i], 1, V[i], CV_GEMM_A_T);

                    //eb_i += BijT * e_ij
                    int index = ((int*)(Vis_index->data.ptr + i * Vis_index->step))[j];

                    CvMat eij;
                    cvGetSubRect(err, &eij, cvRect(0, index, 1, Bij->height /*width of transposed Bij*/));
                    cvGEMM(Bij, &eij, 1, eb[i], 1, eb[i], CV_GEMM_A_T);
                }
            }
        } //V_i and eb_i computed for all i

        //compute W_ij
        for (int i = 0; i < num_points; i++) {
            for (int j = 0; j < num_cams; j++) {
                CvMat* Aij = ((CvMat**)(A->data.ptr + A->step * i))[j];
                if (Aij) {   //visible
                    CvMat* Bij = ((CvMat**)(B->data.ptr + B->step * i))[j];
                    CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];

                    //multiply
                    cvGEMM(Aij, Bij, 1, NULL, 0, Wij, CV_GEMM_A_T);
                }
            }
        } //Wij computed

        //backup diagonal of JtJ before we start augmenting it
        {
            CvMat dia;
            CvMat subr;
            for (int j = 0; j < num_cams; j++) {
                cvGetDiag(U[j], &dia);
                cvGetSubRect(JtJ_diag, &subr,
                             cvRect(0, j * num_cam_param, 1, num_cam_param));
                cvCopy(&dia, &subr);
            }
            for (int i = 0; i < num_points; i++) {
                cvGetDiag(V[i], &dia);
                cvGetSubRect(JtJ_diag, &subr,
                             cvRect(0, num_cams * num_cam_param + i * num_point_param, 1, num_point_param));
                cvCopy(&dia, &subr);
            }
        }

        if (iters == 0) {
            //initialize lambda. It is set to 1e-3 * average diagonal element in JtJ
            double average_diag = 0;
            for (int j = 0; j < num_cams; j++) {
                average_diag += cvTrace(U[j]).val[0];
            }
            for (int i = 0; i < num_points; i++) {
                average_diag += cvTrace(V[i]).val[0];
            }
            average_diag /= (num_cams * num_cam_param + num_points * num_point_param);

            lambda = 1e-3 * average_diag;
        }

        //now we are going to find good step and make it
        for (;;) {
            //augmentation of diagonal
            for (int j = 0; j < num_cams; j++) {
                CvMat diag;
                cvGetDiag(U[j], &diag);
#if 1
                cvAddS(&diag, cvScalar(lambda), &diag);
#else
                cvScale(&diag, &diag, 1 + lambda);
#endif
            }
            for (int i = 0; i < num_points; i++) {
                CvMat diag;
                cvGetDiag(V[i], &diag);
#if 1
                cvAddS(&diag, cvScalar(lambda), &diag);
#else
                cvScale(&diag, &diag, 1 + lambda);
#endif
            }
            bool error = false;
            //compute inv(V*)
            bool inverted_ok = true;
            for (int i = 0; i < num_points; i++) {
                double det = cvInvert(V[i], inv_V_star[i]);

                if (fabs(det) <= FLT_EPSILON) {
                    inverted_ok = false;
                    break;
                } //means we did wrong augmentation, try to choose different lambda
            }

            if (inverted_ok) {
                cvSetZero(E);
                //loop through cameras, compute upper diagonal blocks of matrix S
                for (int j = 0; j < num_cams; j++) {
                    //compute Yij = Wij (V*_i)^-1  for all i   (if Wij exists/nonzero)
                    for (int i = 0; i < num_points; i++) {
                        //
                        CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
                        if (Wij) {
                            cvMatMul(Wij, inv_V_star[i], Yj[i]);
                        }
                    }

                    //compute Sjk   for k>=j  (because Sjk = Skj)
                    for (int k = j; k < num_cams; k++) {
                        cvSetZero(YWt);
                        for (int i = 0; i < num_points; i++) {
                            //check that both Wij and Wik exist
                            CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
                            CvMat* Wik = ((CvMat**)(W->data.ptr + W->step * k))[i];

                            if (Wij && Wik) {
                                //multiply YWt += Yj[i]*Wik'
                                cvGEMM(Yj[i], Wik, 1, YWt, 1, YWt, CV_GEMM_B_T /*transpose Wik*/);
                            }
                        }

                        //copy result to matrix S

                        CvMat Sjk;
                        //extract submat
                        cvGetSubRect(S, &Sjk, cvRect(k * num_cam_param, j * num_cam_param, num_cam_param, num_cam_param));


                        //if j==k, add diagonal
                        if (j != k) {
                            //just copy with minus
                            cvScale(YWt, &Sjk, -1);   //if we set initial S to zero then we can use cvSub( Sjk, YWt, Sjk);
                        } else {
                            //add diagonal value

                            //subtract YWt from augmented Uj
                            cvSub(U[j], YWt, &Sjk);
                        }
                    }

                    //compute right part of equation involving matrix S
                    // e_j=ea_j - \sum_i Y_ij eb_i
                    {
                        CvMat e_j;

                        //select submat
                        cvGetSubRect(E, &e_j, cvRect(0, j * num_cam_param, 1, num_cam_param));

                        for (int i = 0; i < num_points; i++) {
                            CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];
                            if (Wij) {
                                cvMatMulAdd(Yj[i], eb[i], &e_j, &e_j);
                            }
                        }

                        cvSub(ea[j], &e_j, &e_j);
                    }

                }
                //fill below diagonal elements of matrix S
                cvCompleteSymm(S,  0 /*from upper to low*/);   //operation may be done by nonzero blocks or during upper diagonal computation

                //Solve linear system  S * deltaP_a = E
                CvMat dpa;
                cvGetSubRect(deltaP, &dpa, cvRect(0, 0, 1, S->width));
                int res = cvSolve(S, E, &dpa);

                if (res) {   //system solved ok
                    //compute db_i
                    for (int i = 0; i < num_points; i++) {
                        CvMat dbi;
                        cvGetSubRect(deltaP, &dbi, cvRect(0, dpa.height + i * num_point_param, 1, num_point_param));

                        /* compute \sum_j W_ij^T da_j */
                        for (int j = 0; j < num_cams; j++) {
                            //get Wij
                            CvMat* Wij = ((CvMat**)(W->data.ptr + W->step * j))[i];

                            if (Wij) {
                                //get da_j
                                CvMat daj;
                                cvGetSubRect(&dpa, &daj, cvRect(0, j * num_cam_param, 1, num_cam_param));
                                cvGEMM(Wij, &daj, 1, &dbi, 1, &dbi, CV_GEMM_A_T /* transpose Wij */);
                            }
                        }
                        //finalize dbi
                        cvSub(eb[i], &dbi, &dbi);
                        cvMatMul(inv_V_star[i], &dbi, &dbi);   //here we get final dbi
                    }  //now we computed whole deltaP

                    //add deltaP to delta
                    cvAdd(prevP, deltaP, P);

                    //evaluate  function with new parameters
                    ask_for_proj(); // func( P, hX );

                    //compute error
                    errNorm = cvNorm(X, hX, CV_L2);

                } else {
                    error = true;
                }
            } else {
                error = true;
            }
            //check solution
            if (error ||  /* singularities somewhere */
                    errNorm > prevErrNorm) {  //step was not accepted
                //increase lambda and reject change
                lambda *= 10;

                //restore diagonal from backup
                {
                    CvMat dia;
                    CvMat subr;
                    for (int j = 0; j < num_cams; j++) {
                        cvGetDiag(U[j], &dia);
                        cvGetSubRect(JtJ_diag, &subr,
                                     cvRect(0, j * num_cam_param, 1, num_cam_param));
                        cvCopy(&subr, &dia);
                    }
                    for (int i = 0; i < num_points; i++) {
                        cvGetDiag(V[i], &dia);
                        cvGetSubRect(JtJ_diag, &subr,
                                     cvRect(0, num_cams * num_cam_param + i * num_point_param, 1, num_point_param));
                        cvCopy(&subr, &dia);
                    }
                }
            } else { //all is ok
                //accept change and decrease lambda
                lambda /= 10;
                lambda = MAX(lambda, 1e-16);
                prevErrNorm = errNorm;

                //compute new projection error vector
                cvSub(X, hX, err);
                break;
            }
        }
        iters++;

        double param_change_norm = cvNorm(P, prevP, CV_RELATIVE_L2);
        //check termination criteria
        if ((criteria.type & CV_TERMCRIT_ITER && iters > criteria.max_iter) ||
                (criteria.type & CV_TERMCRIT_EPS && param_change_norm < criteria.epsilon)) {
            done = true;
            break;
        } else {
            //copy new params and continue iterations
            cvCopy(P, prevP);
        }
    }
    cvReleaseMat(&YWt);
    cvReleaseMat(&E);
}

//Utilities

void fjac(int /*i*/, int /*j*/, CvMat* point_params, CvMat* cam_params, CvMat* A, CvMat* B, void* /*data*/) {
    //compute jacobian per camera parameters (i.e. Aij)
    //take i-th point 3D current coordinates

    CvMat _Mi;
    cvReshape(point_params, &_Mi, 3, 1);

    CvMat* _mp = cvCreateMat(1, 2, CV_64F);  //projection of the point

    //split camera params into different matrices
    CvMat _ri, _ti, _k;
    cvGetRows(cam_params, &_ri, 0, 3);
    cvGetRows(cam_params, &_ti, 3, 6);

    double intr_data[9] = {0, 0, 0, 0, 0, 0, 0, 0, 1};
    intr_data[0] = cam_params->data.db[6];
    intr_data[4] = cam_params->data.db[7];
    intr_data[2] = cam_params->data.db[8];
    intr_data[5] = cam_params->data.db[9];

    CvMat matA = cvMat(3, 3, CV_64F, intr_data);

    CvMat _dpdr, _dpdt, _dpdf, _dpdc, _dpdk;

    bool have_dk = cam_params->height - 10 ? true : false;

    cvGetCols(A, &_dpdr, 0, 3);
    cvGetCols(A, &_dpdt, 3, 6);
    cvGetCols(A, &_dpdf, 6, 8);
    cvGetCols(A, &_dpdc, 8, 10);

    if (have_dk) {
        cvGetRows(cam_params, &_k, 10, cam_params->height);
        cvGetCols(A, &_dpdk, 10, A->width);
    }
    cvProjectPoints2(&_Mi, &_ri, &_ti, &matA, have_dk ? &_k : NULL, _mp, &_dpdr, &_dpdt,
                     &_dpdf, &_dpdc, have_dk ? &_dpdk : NULL, 0);

    cvReleaseMat(&_mp);

    //compute jacobian for point params
    //compute dMeasure/dPoint3D

    // x = (r11 * X + r12 * Y + r13 * Z + t1)
    // y = (r21 * X + r22 * Y + r23 * Z + t2)
    // z = (r31 * X + r32 * Y + r33 * Z + t3)

    // x' = x/z
    // y' = y/z

    //d(x') = ( dx*z - x*dz)/(z*z)
    //d(y') = ( dy*z - y*dz)/(z*z)

    //g = 1 + k1*r_2 + k2*r_4 + k3*r_6
    //r_2 = x'*x' + y'*y'

    //d(r_2) = 2*x'*dx' + 2*y'*dy'

    //dg = k1* d(r_2) + k2*2*r_2*d(r_2) + k3*3*r_2*r_2*d(r_2)

    //x" = x'*g + 2*p1*x'*y' + p2(r_2+2*x'_2)
    //y" = y'*g + p1(r_2+2*y'_2) + 2*p2*x'*y'

    //d(x") = d(x') * g + x' * d(g) + 2*p1*( d(x')*y' + x'*dy) + p2*(d(r_2) + 2*2*x'* dx')
    //d(y") = d(y') * g + y' * d(g) + 2*p2*( d(x')*y' + x'*dy) + p1*(d(r_2) + 2*2*y'* dy')

    // u = fx*( x") + cx
    // v = fy*( y") + cy

    // du = fx * d(x")  = fx * ( dx*z - x*dz)/ (z*z)
    // dv = fy * d(y")  = fy * ( dy*z - y*dz)/ (z*z)

    // dx/dX = r11,  dx/dY = r12, dx/dZ = r13
    // dy/dX = r21,  dy/dY = r22, dy/dZ = r23
    // dz/dX = r31,  dz/dY = r32, dz/dZ = r33

    // du/dX = fx*(r11*z-x*r31)/(z*z)
    // du/dY = fx*(r12*z-x*r32)/(z*z)
    // du/dZ = fx*(r13*z-x*r33)/(z*z)

    // dv/dX = fy*(r21*z-y*r31)/(z*z)
    // dv/dY = fy*(r22*z-y*r32)/(z*z)
    // dv/dZ = fy*(r23*z-y*r33)/(z*z)

    //get rotation matrix
    double R[9], t[3], fx = intr_data[0], fy = intr_data[4];
    CvMat matR = cvMat(3, 3, CV_64F, R);
    cvRodrigues2(&_ri, &matR);

    double X, Y, Z;
    X = point_params->data.db[0];
    Y = point_params->data.db[1];
    Z = point_params->data.db[2];

    t[0] = _ti.data.db[0];
    t[1] = _ti.data.db[1];
    t[2] = _ti.data.db[2];

    //compute x,y,z
    double x = R[0] * X + R[1] * Y + R[2] * Z + t[0];
    double y = R[3] * X + R[4] * Y + R[5] * Z + t[1];
    double z = R[6] * X + R[7] * Y + R[8] * Z + t[2];

#if 1
    //compute x',y'
    double x_strike = x / z;
    double y_strike = y / z;
    //compute dx',dy'  matrix
    //
    //    dx'/dX  dx'/dY dx'/dZ    =
    //    dy'/dX  dy'/dY dy'/dZ

    double coeff[6] = { z, 0, -x,
                        0, z, -y
                      };
    CvMat coeffmat = cvMat(2, 3, CV_64F, coeff);

    CvMat* dstrike_dbig = cvCreateMat(2, 3, CV_64F);
    cvMatMul(&coeffmat, &matR, dstrike_dbig);
    cvScale(dstrike_dbig, dstrike_dbig, 1 / (z * z));

    if (have_dk) {
        double strike_[2] = {x_strike, y_strike};
        CvMat strike = cvMat(1, 2, CV_64F, strike_);

        //compute r_2
        double r_2 = x_strike * x_strike + y_strike * y_strike;
        double r_4 = r_2 * r_2;
        double r_6 = r_4 * r_2;

        //compute d(r_2)/dbig
        CvMat* dr2_dbig = cvCreateMat(1, 3, CV_64F);
        cvMatMul(&strike, dstrike_dbig, dr2_dbig);
        cvScale(dr2_dbig, dr2_dbig, 2);

        double& k1 = _k.data.db[0];
        double& k2 = _k.data.db[1];
        double& p1 = _k.data.db[2];
        double& p2 = _k.data.db[3];
        double k3 = 0;

        if (_k.cols* _k.rows == 5) {
            k3 = _k.data.db[4];
        }
        //compute dg/dbig
        double dg_dr2 = k1 + k2 * 2 * r_2 + k3 * 3 * r_4;
        double g = 1 + k1 * r_2 + k2 * r_4 + k3 * r_6;

        CvMat* dg_dbig = cvCreateMat(1, 3, CV_64F);
        cvScale(dr2_dbig, dg_dbig, dg_dr2);

        CvMat* tmp = cvCreateMat(2, 3, CV_64F);
        CvMat* dstrike2_dbig = cvCreateMat(2, 3, CV_64F);

        double c[4] = { g + 2 * p1* y_strike + 4 * p2* x_strike,       2 * p1* x_strike,
                        2 * p2* y_strike,                 g + 2 * p2* x_strike + 4 * p1* y_strike
                      };

        CvMat coeffmat = cvMat(2, 2, CV_64F, c);

        cvMatMul(&coeffmat, dstrike_dbig, dstrike2_dbig);

        cvGEMM(&strike, dg_dbig, 1, NULL, 0, tmp, CV_GEMM_A_T);
        cvAdd(dstrike2_dbig, tmp, dstrike2_dbig);

        double p[2] = { p2, p1 };
        CvMat pmat = cvMat(2, 1, CV_64F, p);

        cvMatMul(&pmat, dr2_dbig , tmp);
        cvAdd(dstrike2_dbig, tmp, dstrike2_dbig);

        cvCopy(dstrike2_dbig, B);

        cvReleaseMat(&dr2_dbig);
        cvReleaseMat(&dg_dbig);

        cvReleaseMat(&tmp);
        cvReleaseMat(&dstrike2_dbig);
        cvReleaseMat(&tmp);
    } else {
        cvCopy(dstrike_dbig, B);
    }
    //multiply by fx, fy
    CvMat row;
    cvGetRows(B, &row, 0, 1);
    cvScale(&row, &row, fx);

    cvGetRows(B, &row, 1, 2);
    cvScale(&row, &row, fy);

#else

    double k = fx / (z * z);

    cvmSet(B, 0, 0, k * (R[0]*z - x * R[6]));
    cvmSet(B, 0, 1, k * (R[1]*z - x * R[7]));
    cvmSet(B, 0, 2, k * (R[2]*z - x * R[8]));

    k = fy / (z * z);

    cvmSet(B, 1, 0, k * (R[3]*z - y * R[6]));
    cvmSet(B, 1, 1, k * (R[4]*z - y * R[7]));
    cvmSet(B, 1, 2, k * (R[5]*z - y * R[8]));

#endif

};
void func(int /*i*/, int /*j*/, CvMat* point_params, CvMat* cam_params, CvMat* estim, void* /*data*/) {
    //just do projections
    CvMat _Mi;
    cvReshape(point_params, &_Mi, 3, 1);

    CvMat* _mp = cvCreateMat(1, 2, CV_64F);  //projection of the point

    //split camera params into different matrices
    CvMat _ri, _ti, _k;

    cvGetRows(cam_params, &_ri, 0, 3);
    cvGetRows(cam_params, &_ti, 3, 6);

    double intr_data[9] = {0, 0, 0, 0, 0, 0, 0, 0, 1};
    intr_data[0] = cam_params->data.db[6];
    intr_data[4] = cam_params->data.db[7];
    intr_data[2] = cam_params->data.db[8];
    intr_data[5] = cam_params->data.db[9];

    CvMat matA = cvMat(3, 3, CV_64F, intr_data);

    //int cn = CV_MAT_CN(_Mi.type);

    bool have_dk = cam_params->height - 10 ? true : false;

    if (have_dk) {
        cvGetRows(cam_params, &_k, 10, cam_params->height);
    }
    cvProjectPoints2(&_Mi, &_ri, &_ti, &matA, have_dk ? &_k : NULL, _mp, NULL, NULL,
                     NULL, NULL, NULL, 0);
    cvTranspose(_mp, estim);
    cvReleaseMat(&_mp);
};

void fjac_new(int i, int j, Mat& point_params, Mat& cam_params, Mat& A, Mat& B, void* data) {
    CvMat _point_params = point_params, _cam_params = cam_params, matA = A, matB = B;
    fjac(i, j, &_point_params, &_cam_params, &matA, &matB, data);
};

void func_new(int i, int j, Mat& point_params, Mat& cam_params, Mat& estim, void* data) {
    CvMat _point_params = point_params, _cam_params = cam_params, _estim = estim;
    func(i, j, &_point_params, &_cam_params, &_estim, data);
};

void LevMarqSparse::bundleAdjust(vector<Point3d>& points,  //positions of points in global coordinate system (input and output)
                                 const vector<vector<Point2d> >& imagePoints, //projections of 3d points for every camera
                                 const vector<vector<int> >& visibility, //visibility of 3d points for every camera
                                 vector<Mat>& cameraMatrix, //intrinsic matrices of all cameras (input and output)
                                 vector<Mat>& R, //rotation matrices of all cameras (input and output)
                                 vector<Mat>& T, //translation vector of all cameras (input and output)
                                 vector<Mat>& distCoeffs, //distortion coefficients of all cameras (input and output)
                                 const TermCriteria& criteria)
//,enum{MOTION_AND_STRUCTURE,MOTION,STRUCTURE})
{
    int num_points = points.size();
    int num_cameras = cameraMatrix.size();

    CV_Assert(imagePoints.size() == (size_t)num_cameras &&
              visibility.size() == (size_t)num_cameras &&
              R.size() == (size_t)num_cameras &&
              T.size() == (size_t)num_cameras &&
              (distCoeffs.size() == (size_t)num_cameras || distCoeffs.size() == 0));

    int numdist = distCoeffs.size() ? (distCoeffs[0].rows * distCoeffs[0].cols) : 0;

    int num_cam_param = 3 /* rotation vector */ + 3 /* translation vector */
                        + 2 /* fx, fy */ + 2 /* cx, cy */ + numdist;

    int num_point_param = 3;

    //collect camera parameters into vector
    Mat params(num_cameras * num_cam_param + num_points * num_point_param, 1, CV_64F);

    //fill camera params
    for (int i = 0; i < num_cameras; i++) {
        //rotation
        Mat rot_vec; Rodrigues(R[i], rot_vec);
        Mat dst = params.rowRange(i * num_cam_param, i * num_cam_param + 3);
        rot_vec.copyTo(dst);

        //translation
        dst = params.rowRange(i * num_cam_param + 3, i * num_cam_param + 6);
        T[i].copyTo(dst);

        //intrinsic camera matrix
        double* intr_data = (double*)cameraMatrix[i].data;
        double* intr = (double*)(params.data + params.step * (i * num_cam_param + 6));
        //focals
        intr[0] = intr_data[0];  //fx
        intr[1] = intr_data[4];  //fy
        //center of projection
        intr[2] = intr_data[2];  //cx
        intr[3] = intr_data[5];  //cy

        //add distortion if exists
        if (distCoeffs.size()) {
            dst = params.rowRange(i * num_cam_param + 10, i * num_cam_param + 10 + numdist);
            distCoeffs[i].copyTo(dst);
        }
    }

    //fill point params
    Mat ptparams(num_points, 1, CV_64FC3, params.data + num_cameras * num_cam_param * params.step);
    Mat _points(points);
    CV_Assert(_points.size() == ptparams.size() && _points.type() == ptparams.type());
    _points.copyTo(ptparams);

    //convert visibility vectors to visibility matrix
    Mat vismat(num_points, num_cameras, CV_32S);
    for (int i = 0; i < num_cameras; i++) {
        //get row
        Mat col = vismat.col(i);
        Mat((int)visibility[i].size(), 1, vismat.type(), (void*)&visibility[i][0]).copyTo(col);
    }

    int num_proj = countNonZero(vismat); //total number of points projections

    //collect measurements
    Mat X(num_proj * 2, 1, CV_64F); //measurement vector

    int counter = 0;
    for (int i = 0; i < num_points; i++) {
        for (int j = 0; j < num_cameras; j++) {
            //check visibility
            if (visibility[j][i]) {
                //extract point and put tu vector
                Point2d p = imagePoints[j][i];
                ((double*)(X.data))[counter] = p.x;
                ((double*)(X.data))[counter + 1] = p.y;
                counter += 2;
            }
        }
    }

    LevMarqSparse levmar(num_points, num_cameras, num_point_param, num_cam_param, 2, vismat, params, X,
                         TermCriteria(criteria), fjac_new, func_new, NULL);
    //extract results
    //fill point params
    Mat final_points(num_points, 1, CV_64FC3,
                     levmar.P->data.db + num_cameras * num_cam_param * levmar.P->step);
    CV_Assert(_points.size() == final_points.size() && _points.type() == final_points.type());
    final_points.copyTo(_points);

    //fill camera params
    for (int i = 0; i < num_cameras; i++) {
        //rotation
        Mat rot_vec = Mat(levmar.P).rowRange(i * num_cam_param, i * num_cam_param + 3);
        Rodrigues(rot_vec, R[i]);
        //translation
        T[i] = Mat(levmar.P).rowRange(i * num_cam_param + 3, i * num_cam_param + 6);

        //intrinsic camera matrix
        double* intr_data = (double*)cameraMatrix[i].data;
        double* intr = (double*)(Mat(levmar.P).data + Mat(levmar.P).step * (i * num_cam_param + 6));
        //focals
        intr_data[0] = intr[0];  //fx
        intr_data[4] = intr[1];  //fy
        //center of projection
        intr_data[2] = intr[2];  //cx
        intr_data[5] = intr[3];  //cy

        //add distortion if exists
        if (distCoeffs.size()) {
            params.rowRange(i * num_cam_param + 10, i * num_cam_param + 10 + numdist).copyTo(distCoeffs[i]);
        }
    }
}

}// end of namespace cv
